#include <stdio.h>
#include <avr/io.h>
#include <avr/interrupt.h>

typedef unsigned char byte;

byte uartBuf[32];
volatile byte uartCnt;
char hexChars[]="0123456789ABCDEF";

#define TW_START() (TWCR=((1<<TWEN)|(1<<TWSTA)|(1<<TWINT)))
#define TW_STOP() (TWCR=((1<<TWEN)|(1<<TWSTO)|(1<<TWINT)))
#define TW_INT() (TWCR&(1<<TWINT))
#define TW_STATUS() (TWSR&0xF8)
#define TW_ADDR_WR(a) (TWDR=(a<<1))
#define TW_ADDR_RD(a) (TWDR=(a<<1)|1)
#define TW_SEND() (TWCR=((1<<TWEN)|(1<<TWINT)|(1<<TWEA)))
#define TW_RCV_NA() (TWCR=((1<<TWEN)|(1<<TWINT)))
#define TW_RCV_AK() (TWCR=((1<<TWEN)|(1<<TWINT)|(1<<TWEA)))
#define TW_DATA(d) (TWDR=d)
#define TW_GET() (TWDR)

void
SetupHardware(void){
  DDRC=0x00;
  PORTC=0x00;
  DDRB=0x01;
  DDRD=0x02;
  PORTD=0x01;
  TWBR=0x0A;
  TWCR=(1<<TWEN);
  TWSR=0x00;
  UBRRH=0x00;
  UBRRL=0x33;//9600 8n1 with U2X
  UCSRA=0x02;
  UCSRB=0x48;//transmitter on and trans int on
  UCSRC=0x86;
  MCUCR=0x10;
  uartCnt=sizeof(uartBuf);
  sei();
}//SetupHardware

ISR(USART_TXC_vect){
  if(uartCnt<sizeof(uartBuf)){
    if(uartBuf[uartCnt])
      UDR=uartBuf[uartCnt++];
    else
      uartCnt=sizeof(uartBuf);
  }//if
}//ISR(USART_TXC_vect)

void
UartString(char*s){
  byte i;
  while(uartCnt!=sizeof(uartBuf))
    asm volatile ("nop");
  i=0;
  do{
    uartBuf[i]=s[i];
  }while(s[i++]);
  UDR=uartBuf[0];
  uartCnt=1;
}//UartString

void
UartHex(byte d){
  while(uartCnt!=sizeof(uartBuf))
    asm volatile ("nop");
  uartBuf[0]=hexChars[d>>4];
  uartBuf[1]=hexChars[d&0xF];
  uartBuf[2]=0;
  UDR=uartBuf[0];
  uartCnt=1;
}//UartHex

short
TwRead(byte a){
  byte d=0;
  short e;
  e=-15;
  TW_START();
  while(!TW_INT())
    asm volatile ("nop");
  if(TW_STATUS()!=0x08){
    e=-1;
    goto error;
  }//if
  TW_ADDR_WR(0x1D);
  TW_SEND();
  while(!TW_INT())
    asm volatile ("nop");
  if(TW_STATUS()!=0x18){
    e=-2;
    goto error;
  }//if
  TW_DATA(a);
  TW_SEND();
  while(!TW_INT())
    asm volatile ("nop");
  if(TW_STATUS()!=0x28){
    e=-3;
    goto error;
  }//if
  TW_START();
  while(!TW_INT())
    asm volatile ("nop");
  if(TW_STATUS()!=0x10){
    e=-4;
    goto error;
  }//if
  TW_ADDR_RD(0x1D);
  TW_SEND();
  while(!TW_INT())
    asm volatile ("nop");
  if(TW_STATUS()!=0x40){
    e=-5;
    goto error;
  }//if
  TW_RCV_NA();
  while(!TW_INT())
    asm volatile ("nop");
  if(TW_STATUS()!=0x58){
    e=-6;
    goto error;
  }//if
  d=TW_GET();
  e=0;
  error:
  TW_STOP();
  if(e==0)
    return d;
  return e;
}//TwRead

short
TwReadSeq(byte a,byte cnt,byte*res){
  byte d=0,i=0;
  short e;
  cnt--;
  e=-15;
  TW_START();
  while(!TW_INT())
    asm volatile ("nop");
  if(TW_STATUS()!=0x08){
    e=-1;
    goto error;
  }//if
  TW_ADDR_WR(0x1D);
  TW_SEND();
  while(!TW_INT())
    asm volatile ("nop");
  if(TW_STATUS()!=0x18){
    e=-2;
    goto error;
  }//if
  TW_DATA(a);
  TW_SEND();
  while(!TW_INT())
    asm volatile ("nop");
  if(TW_STATUS()!=0x28){
    e=-3;
    goto error;
  }//if
  TW_START();
  while(!TW_INT())
    asm volatile ("nop");
  if(TW_STATUS()!=0x10){
    e=-4;
    goto error;
  }//if
  TW_ADDR_RD(0x1D);
  TW_SEND();
  while(!TW_INT())
    asm volatile ("nop");
  if(TW_STATUS()!=0x40){
    e=-5;
    goto error;
  }//if
  next:
  if(i<cnt)
    TW_RCV_AK();
  else
    TW_RCV_NA();
  while(!TW_INT())
    asm volatile ("nop");
  if((TW_STATUS()|0x08)!=0x58){
    e=-6;
    goto error;
  }//if
  res[i]=TW_GET();
  e=0;
  if(i++<cnt)
    goto next;
  error:
  TW_STOP();
  if(e==0)
    return d;
  return e;
}//TwReadSeq

short
TwWrite(byte a,byte d){
  short e;
  e=-15;
  TW_START();
  while(!TW_INT())
    asm volatile ("nop");
  if(TW_STATUS()!=0x08){
    e=-1;
    goto error;
  }//if
  TW_ADDR_WR(0x1D);
  TW_SEND();
  while(!TW_INT())
    asm volatile ("nop");
  if(TW_STATUS()!=0x18){
    e=-2;
    goto error;
  }//if
  TW_DATA(a);
  TW_SEND();
  while(!TW_INT())
    asm volatile ("nop");
  if(TW_STATUS()!=0x28){
    e=-3;
    goto error;
  }//if
  TW_DATA(d);
  TW_SEND();
  while(!TW_INT())
    asm volatile ("nop");
  if(TW_STATUS()!=0x28){
    e=-4;
    goto error;
  }//if
  UartString("Write2\r");
  e=0;
  error:
  TW_STOP();
  return e;
}//TwWrite

int
main(void){
  short res;
  byte g[4];
  byte stat;
  signed char gx,gy;
  signed short sx=0,sy=0;
  byte i;
  SetupHardware();
  UartString("Program started!\r\n");
  res=TwWrite(0x16,0x05);
  UartString("Setup: ");
  UartHex(res>>8);
  UartHex(res&0xFF);
  UartString("\r\n");
  
  i=0;
  repeat:
  while((PIND&0x04)==0)
    asm volatile ("nop");
  TwReadSeq(0x06,4,g);
  gx=(signed char)g[0];
  gy=(signed char)g[2];
  stat|=g[3];
  gx+=0x10;
  gy+=0;
  sx+=gx;
  sy+=gy;
  if(++i<8)
    goto repeat;
  sx>>=3;
  sy>>=3;
  UartString("       \r");
  UartHex((byte)sx);
  UartHex((byte)sy);
  UartString(" ");
  UartHex(g[3]);
  i=0;
  sx=0;
  sy=0;
  stat=0;
  while(uartCnt!=sizeof(uartBuf))
    asm volatile ("nop");
  TwReadSeq(0x06,4,g);
  goto repeat;
}//main
